%function process_results(experiment_out, piccolo)
function process_results()
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%

%	datadir='saved_data/2010-09-29/.logs/09_56_05/';
	datadir='saved_data/2010-09-29/.logs/10_20_32/';
%	datadir='saved_data/2010-09-29/.logs/10_32_12/';
%	datadir='saved_data/2010-09-29/.logs/10_41_58/';
	load ([datadir 'experiment_out.mat']);
	load ([datadir 'piccolo.mat']);

	piccolo = data.Piccolo;

	settings.datadir = '../../data/';
	%settings.dataset='2007-10-25';
	settings.dataset='2009-10-26';
	weather = Weather(settings);

	%FIXME -- perhaps there is a better way to determine when the sim was started and stopped
	piccolo_start = min(find(abs(piccolo.WindSouth_mps_) > 0.20));
	piccolo_end = max(find(abs(piccolo.WindSouth_mps_) > 0.20));
	piccolo_range = piccolo_start:piccolo_end;
	
	piccolo_time = (piccolo.Clock_ms_(piccolo_range)-piccolo.Clock_ms_(piccolo_start))/1000.0;

%-----[ Interpolation ]-----%
  time = piccolo_time;

	p.Ws = piccolo.WindSouth_mps_(piccolo_range);
	p.Ww = piccolo.WindWest_mps_(piccolo_range);
	p.Lon = 180/pi * piccolo.Lon_rad_(piccolo_range);
	p.Lat = 180/pi * piccolo.Lat_rad_(piccolo_range);
	p.Alt = piccolo.Height_m_(piccolo_range);
	p.P = piccolo.P_radps_(piccolo_range);
  p.Loop.Alt = piccolo.LoopTarget1_(piccolo_range);
  p.Loop.IAS = piccolo.LoopTarget0_(piccolo_range);
	p.Elevator = piccolo.Surface0_(piccolo_range);
	p.Aileron  = piccolo.Surface1_(piccolo_range);
	p.Throttle = piccolo.Surface2_(piccolo_range);
	p.Rudder   = piccolo.Surface3_(piccolo_range);

	gps  = experiment_out.find('logsout').Sensors.('GPS Convert').gps;
	gps_time  = experiment_out.find('logsout').Sensors.('GPS Convert').gps.unnamed1.Time;
	imu  = experiment_out.find('logsout').Sensors.('IMU Convert').imu;
	air  = experiment_out.find('logsout').Sensors.('Air Data Convert').airdata;
	air_time  = experiment_out.find('logsout').Sensors.('Air Data Convert').airdata.unnamed1.Time;
	wind  = experiment_out.find('logsout').Winds.wind;
	wind_time  = experiment_out.find('logsout').Winds.wind.unnamed1.Time;

	s.Lat = interp1(gps_time,gps.unnamed1.Data,piccolo_time);
	s.Lon = interp1(gps_time,gps.unnamed2.Data,piccolo_time);
	s.Alt = interp1(gps_time,gps.unnamed3.Data,piccolo_time);
	s.Wx = interp1(wind_time,wind.unnamed1.Data,piccolo_time);
	s.Wy = interp1(wind_time,wind.unnamed2.Data,piccolo_time);
	s.Wz = interp1(wind_time,wind.unnamed3.Data,piccolo_time);
	s.Airspeed = interp1(air_time,air.unnamed1.Data,piccolo_time);
	s.Sideslip = interp1(air_time,air.unnamed4.Data,piccolo_time);
	s.AOA = interp1(air_time,air.unnamed5.Data,piccolo_time);

%-----[ Winds ]-----%
	figure(1); clf;


	plot(wind.unnamed1,'r','LineWidth',2.0);
	hold on;
	plot(wind.unnamed2,'g','LineWidth',2.0);
	plot(wind.unnamed3,'b','LineWidth',2.0);

	for i = 1:14
		plot([60*i,60*i],[-25,25],'k:');
	end

	legend('Wx','Wy','Wz');
	xlabel('Time (s)');
	ylabel('Magnitude (m/s)');
	title('Wind Values');

%-----[ Measured vs Actual Winds ]-----%
	figure(2); clf;

	scale = 1;
	hold on;
	plot(p.Lon, p.Lat,'.-');
	step = 350;
	quiver(p.Lon(1:step:end), p.Lat(1:step:end), p.Ww(1:step:end), p.Ws(1:step:end),0.5,'LineWidth',2);
	quiver(s.Lon(1:step:end), s.Lat(1:step:end), s.Wx(1:step:end), s.Wy(1:step:end),0.5,'LineWidth',2);
	hold off;

	legend('Track','Measured Winds','Actual Winds');
	xlabel('Lon (deg)');
	ylabel('Lat (deg)');
	title('Winds');
%-----[ Measured vs Actual Winds ]-----%
	figure(3); clf;

	p.Wdir = atan2(p.Ww,p.Ws);
	for i = 2:length(p.Wdir)
	diff = p.Wdir(i)-p.Wdir(i-1);
	if abs(diff) > pi 
		mag = round(diff/(2*pi));
		p.Wdir(i) = p.Wdir(i) - 2*pi*mag;
	end
	end

	p.Wdir = p.Wdir.*180./pi;
	p.Wmag = sqrt((p.Ww.*p.Ww) + (p.Ws.*p.Ws));

	s.Wdir = atan2(s.Wx,s.Wy);
	for i = 2:length(s.Wdir)
	diff = s.Wdir(i)-s.Wdir(i-1);
	if abs(diff) > pi 
		mag = round(diff/(2*pi));
		s.Wdir(i) = s.Wdir(i) - 2*pi*mag;
	end
	end

	s.Wdir = s.Wdir.*180./pi;
	s.Wmag = sqrt((s.Wx.*s.Wx) + (s.Wy.*s.Wy));

	subplot(2,1,1);
	plot(time,p.Wdir,'b','LineWidth',2); 
	hold on;
	plot(time,s.Wdir,'r','LineWidth',2); 
	hold off;
	legend('Measured','Actual');
	ylabel('Direction (deg)');
	title('Wind Direction');
	subplot(2,1,2);
	plot(time,p.Wmag,'b','LineWidth',2); 
	hold on;
	plot(time,s.Wmag,'r','LineWidth',2); 
	hold off;
	legend('Measured','Actual');
	ylabel('Magnitude (m/s)');
	xlabel('Time (s)');
	title('Wind Speed');
%-----[ Wind Estimate Error ]-----%
  figure(4); clf;

	Wdir_error = p.Wdir - s.Wdir;
	for i = 2:length(Wdir_error)
	if abs(Wdir_error(i)) > 180
		mag = round(Wdir_error(i)/(180));
		Wdir_error(i) = Wdir_error(i) - 180*mag;
	end
	end

	P_deg = p.P*180.0/pi;

	poly = polyfit(abs(P_deg),abs(Wdir_error),2);
	f = polyval(poly,sort(abs(P_deg)));
	plot(abs(P_deg(1:2:end)),abs(Wdir_error(1:2:end)),'.');
	hold on;
	plot(sort(abs(P_deg(1:2:end))),f(1:2:end),'r-','LineWidth',2);
	axis([0 30 0 180]);
	xlabel('Turn Rate (deg/s)');
	ylabel('Wind Direction Estimate Error (deg)');
	title('Wind Estimate vs. Turn Rate');

%-----[ Altitude ]-----%
	figure(5); clf;

	subplot(3,1,1);
	plot(time,p.Loop.Alt,'r','LineWidth',2);
	hold on;
	plot(time,p.Alt,'LineWidth',2.0);
	%plot([0 900],[1550 1550],'k'); % GROUND
	%legend('Alt','Commanded Alt','Ground');
	legend('Commanded Alt','Alt');
	xlabel('Time (s)');
	ylabel('Alt (m)');
	title('UA Altitude');
	subplot(3,1,2);
	plot(time,p.Loop.IAS,'r','LineWidth',2);
	hold on;
	plot(time,s.Airspeed,'LineWidth',2.0);
	legend('Commanded IAS','IAS');
	xlabel('Time (s)');
	ylabel('IAS (m/s)');
	subplot(3,1,3);
	plot(time,p.Throttle,'LineWidth',2.0);
	xlabel('Time (s)');
	ylabel('Throttle(%)');
	ylim([0 1]);

%-----[ Surfaces ]-----%
%	figure(6); clf;
%
%	title('Surface Commands');
%	subplot(4,1,1);
%	plot(p.Elevator,'LineWidth',2.0);
%	legend('Elevator');
%	ylabel('Deflection (rad)');
%
%	subplot(4,1,2);
%	plot(p.Aileron,'LineWidth',2.0);
%	legend('Aileron');
%	ylabel('Deflection (rad)');
%
%	subplot(4,1,3);
%	plot(p.Throttle,'LineWidth',2.0);
%	legend('Throttle');
%	ylabel('%');
%
%	subplot(4,1,4);
%	plot(p.Rudder,'LineWidth',2.0);
%	legend('Rudder');
%	ylabel('Deflection (rad)');
%	xlabel('Time (s)');

%-----[ Airspeed, Slideslip, AOA ]-----%
%	figure(7); clf;
%
%	subplot(3,1,1);
%	plot(s.Airspeed,'LineWidth',2.0);
%	ylabel('TAS (m/s)');
%	subplot(3,1,2);
%	plot(s.Sideslip,'LineWidth',2.0);
%	ylabel('Sideslip (m/s)');
%	subplot(3,1,3);
%	plot(s.AOA,'LineWidth',2.0);
%	ylabel('AOA (deg)');

%-----[ Horizontal Transects ]-----%
%	figure(8); clf;
%	plot(gps.unnamed2.Data,gps.unnamed1.Data);
%	title('UA Track');

%-----[ Vertical Transects ]-----%
	figure(9); clf;
	%plot((gps.unnamed2.Data-gps.unnamed2.Data(1))/0.000009,gps.unnamed3.Data,'LineWidth',2.0);
	%hold on;
	%quiver((gps.unnamed2.Data(1:10:end)-gps.unnamed2.Data(1))/0.000009,gps.unnamed3.Data(1:10:end),-winds.unnamed1.Data(1:10000:end),-winds.unnamed3.Data(1:10000:end));

	dist_from_gs = lon_to_m(s.Lon-p.Lon(1),p.Lat(1));
	plot(dist_from_gs./50,s.Alt,'LineWidth',2.0);
	hold on;
	step = 200;
	quiver(dist_from_gs(1:step:end)./50,s.Alt(1:step:end),s.Wx(1:step:end),-s.Wz(1:step:end).*10,1,'ShowArrowHead','on','LineWidth',2);
	%gf_start = -28;
	%gf_end = -48;
	%gf_start = -60;
	%gf_end = -95;
	%gf_start = -48;
	%gf_end = -78;
	gf_start = -16;
	gf_end = -22;
	plot([gf_start,gf_start],[2520,2600],'k--','LineWidth',2);
	plot([gf_end,gf_end],[2520,2600],'k--','LineWidth',2);
	hold off;

	legend('Track','Wind','Gust Front');
	xlabel('E Distance (50m)');
	ylabel('Alt (m)');
	title('UA Track');
